
#include <ServoPWMF.h>

int LRPin = 0;  
int UDPin = 1;  
int val = 0;       // variable to store the value coming from the sensor
boolean lastLR = false;
boolean cmdAvailable = false;
boolean cmdStarted = false;
unsigned long lastCommandTime=0;
unsigned long lastLRSampleTime=0;
//Time = OriginalTime * 61.03515625 / 976.5625
unsigned int benderSampleRate = 100 * 61.03515625 / 976.5625;
; //100ms cmd sampling rate;
unsigned int cmdMadeDelay = 1500 * 61.03515625 / 976.5625; // after 1500ms if no more cmd, then interpret cmd
int DLRTV;
int RTV;
int LTV;

int DUDTV;
int UTV;


ServoPWMF s;

char* LRCmd="NNNNN";
int LRSize = 5;
int LRCur=0;

void setup() {
  Serial.begin(9600);
  s.setupTimer();
  DLRTV = analogRead(LRPin);
  RTV = DLRTV + 40;
  LTV = DLRTV - 40;
  Serial.print("LRV");
  Serial.println(DLRTV);

  DUDTV = analogRead(UDPin);
  UTV = DUDTV - 100;
}


void loop() {

  sampleBender();
  performAction();

}

//does not perform action if cmdAvailable == false
void performAction()
{
  int UDval = analogRead(UDPin);
  float precent= UDval/-0.5;
    if( cmdAvailable)
  {
    Serial.print("Cmd:");
    Serial.println(LRCmd);

    if(!strcmp(LRCmd, "RNNNN"))
    {
      Serial.println("GRAB");
    }
    if(!strcmp(LRCmd, "RRNNN"))
    {
      Serial.println("PINCH");
    }

    //reset
   // if( UDval > UTV )
    {
      cmdAvailable = false;
      cmdStarted = false;
      for(int i=0; i< LRSize; i++)
      {
        LRCmd[i]='N';
      }
      LRCur=0;

      //   Serial.println(LRCmd);
      //   Serial.println(LRCur);
    }
  }  
}


void sampleBender()
{
  if( ((millis() - lastCommandTime) > cmdMadeDelay  && cmdStarted)   ||  LRCur >=LRSize)
  {
    cmdAvailable = true; 
    return;
  }

  if( (millis() - lastLRSampleTime) > benderSampleRate) //delay 100ms before we take next sample
  {
    val = analogRead(LRPin);    // read the value from the sensor
//u      Serial.println(val);
    if( !lastLR )
    {
      if(val > RTV)
      {
        lastLR = true; 
        lastCommandTime = millis();
        cmdStarted = true;
        LRCmd[LRCur++] = 'R';
        Serial.println("right"); 
        Serial.println(val);
      }
      if(val < LTV)
      {
        lastLR = true;
        lastCommandTime = millis();
        cmdStarted = true;
        LRCmd[LRCur++] = 'L';
        Serial.println("left"); 
        Serial.println(val);
      }
    }
    if(  lastLR&& val< RTV-15 && val > LTV + 15)
    {
      lastLR = false;
      //      Serial.println("middle");
    }

    lastLRSampleTime = millis();
  }

}
